
#include "PandaDump.h"
#include "math.h"

// Constructor
const double pulse = 0.3;

PandaDump::PandaDump(UINT32 upChannel, UINT32 downChannel, Joystick* stick, int dumpButton, unsigned dumpLength) 
{
	m_upSolenoid = new Solenoid(upChannel);
	m_downSolenoid = new Solenoid(downChannel);
	this->m_stick = stick;
	this->m_dumpButton = dumpButton;
	m_state = downOff;
	m_dumpTime = 0;
	m_dumpLength = dumpLength;
	m_dropTime = 0;
}

// Destructor
PandaDump::~PandaDump(void) 
{

}

void PandaDump::Teleop()
{
	double curTime = GetTime();
	switch (m_state)
	{
	case downOff:
		if (m_stick->GetRawButton(m_dumpButton))
		{
			m_upSolenoid->Set(true);
			m_state = upOn;
			m_dumpTime = GetTime();
			printf("Case is upOn at %f\n", curTime);
		}
		break;
	case upOn:
		if (curTime - m_dumpTime > pulse)
		{
			m_upSolenoid->Set(false);
			m_state = upOff;
			printf("Case is upOff at %f\n", curTime);
		}
		break;
	case upOff:
		//if (curTime - m_dumpTime > m_dumpLength)
		if (!m_stick->GetRawButton(m_dumpButton))
		{
			m_downSolenoid->Set(true);
			m_dropTime = GetTime();	
			m_state = downOn;
			printf("Case is downOn at %f\n", curTime);
		}
		break;
		
	case downOn:
		if (curTime - m_dropTime > pulse)
		{
			m_downSolenoid->Set(false);
			m_state = downOff;
			printf("Case is default at %f\n", curTime);
		}
	}
	/*if (curTime - m_dumpTime < m_dumpLength)
		
	if (m_stick->GetRawButton(m_dumpButton))
	{
		m_dumpTime = GetTime();
		float pulseTime = GetTime();
		m_upSolenoid->Set(true);
		bool thing = true;
	}
	else
	{
			m_upSolenoid->Set(false);
			m_downSolenoid->Set(false);
	}
	*/
}

void PandaDump::TestDump(bool setUp, bool setDown)
{
	m_upSolenoid->Set(setUp);
	m_downSolenoid->Set(setDown);
}
